import Project.Main as Main
import RobotClass
import PointClass
import socket
import sys

def isFloat(str):
    try:
        float(str)
    except ValueError:
        return False
    return True

# Checks if all robots were captured
def allRobotsCaptured(rArr):
    for r in rArr:
        if (r == None):
            return False
    return True

# Read input from camera and generate robots structures
def trace():
    robotArray = [None] * Main.NUM_OF_ROBOTS # array of robots  
    
    # UDP server setup
    UDP_IP = "127.0.0.1"
    UDP_PORT = 8008
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.bind((UDP_IP, UDP_PORT))
    
    # Get initial data from camera
    data, addr = sock.recvfrom(1024)
    hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
    
    # Wait for the second <SOH> to start sampling
    while (hex_data != "1"):
        data, addr = sock.recvfrom(1024)
        hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
    data, addr = sock.recvfrom(1024)
    hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
    while (hex_data != "1"):
        data, addr = sock.recvfrom(1024)
        hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
    
    while (not allRobotsCaptured(robotArray)):
        robot = RobotClass.Robot() # initialize new robot
            
        # Get <count>, verify it's an integer and ignore it
        data, addr = sock.recvfrom(1024)
        if (not isFloat(str(data))): # verify we got an integer, exit otherwise
            hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
            sys.exit("ERROR: Received value of <count>, expected integer but received: " + str(data) + ", in hex: " + str(hex_data))
        
        # Get <id number per color> and save it to robot.Id
        data, addr = sock.recvfrom(1024)
        if (not isFloat(str(data))): # verify we got an integer, exit otherwise
            hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
            sys.exit("ERROR: Received value of <id number per color>, expected integer but received: " + str(data) + ", in hex: " + str(hex_data))
        robot.Id = int(data)
        
        # Get <length of contour> and save it to robot.lengthOfContour
        data, addr = sock.recvfrom(1024)
        if (not isFloat(str(data))): # verify we got an integer, exit otherwise
            hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
            sys.exit("ERROR: Received value of <length of contour>, expected integer but received: " + str(data) + ", in hex: " + str(hex_data))
        robot.lengthOfContour = float(data)
        
        # Get <STX> indicating we're about to receive robot's center coordinates
        data, addr = sock.recvfrom(1024)
        hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
        if (hex_data != "2"):
            sys.exit("ERROR: Expected to receive <STX> (2 in hex), but received: " + str(hex_data))
        
        # Get robot center point X coordinate and save it
        centerPoint = PointClass.Point()
        data, addr = sock.recvfrom(1024)
        if (not isFloat(str(data))): # verify we got an integer, exit otherwise
            hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
            sys.exit("ERROR: Received value of robot center point X coordinate, expected integer but received: " + str(data) + ", in hex: " + str(hex_data))
        centerPoint.x = float(data)
        
        # Get <TAB> between x and y coordinates
        data, addr = sock.recvfrom(1024)
        hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
        if (hex_data != "9"):
            sys.exit("ERROR: Expected to receive <TAB> (9 in hex) between center point coordinates, but received: " + str(hex_data))
        
        # Get robot center point Y coordinate and save it. Then, store the point as robot's center
        data, addr = sock.recvfrom(1024)
        if (not isFloat(str(data))): # verify we got an integer, exit otherwise
            hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
            sys.exit("ERROR: Received value of robot center point Y coordinate, expected integer but received: " + str(data) + ", in hex: " + str(hex_data))
        centerPoint.y = float(data)
        robot.center = centerPoint
        
        # Get <GS> indicating we're about to receive contour points
        data, addr = sock.recvfrom(1024)
        hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
        if (hex_data != "1d"):
            sys.exit("ERROR: Expected to receive <GS> (29 [1d] in hex), but received: " + str(hex_data))
            
        # Run until we get <EOT> ('4' in hex_, indicating we're done with this robot
        data, addr = sock.recvfrom(1024)
        hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
        while (hex_data != "4"):
            contourPoint = PointClass.Point() # initialize new point
            if (not isFloat(str(data))): # verify we got an integer, exit otherwise
                hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
                print ("ERROR: Received value of robot contour point X coordinate, expected integer but received: " + str(data) + ", in hex: " + str(hex_data))
                data, addr = sock.recvfrom(1024)
                hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
                print ("next thing is: " + str(data) + ", in hex: " + str(hex_data))
                sys.exit()
                #sys.exit("ERROR: Received value of robot contour point X coordinate, expected integer but received: " + str(data) + ", in hex: " + str(hex_data))
            contourPoint.x = float(data) # set point x coordinate
            
            # Get <TAB> separating between x and y coordinates
            data, addr = sock.recvfrom(1024) 
            hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
            if (hex_data != "9"):
                sys.exit("ERROR: Expected to receive <TAB> (9 in hex) between contour point coordinates, but received: " + str(hex_data))
            
            # set point y coordinate
            data, addr = sock.recvfrom(1024)
            if (not isFloat(str(data))): # verify we got an integer, exit otherwise
                hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
                sys.exit("ERROR: Received value of robot contour point Y coordinate, expected integer but received: " + str(data) + ", in hex: " + str(hex_data))
            contourPoint.y = float(data) # set point x coordinate
            robot.contourPoints.append(contourPoint)
            
            # Get <RS> separating between two contour coordinates
            data, addr = sock.recvfrom(1024) 
            hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
            if (hex_data != "1e"):
                sys.exit("ERROR: Expected to receive <RS> (1e in hex) between contour point coordinates, but received: " + str(hex_data))
            
            # Get next input to check if it's another coordinate or <EOT>
            data, addr = sock.recvfrom(1024)
            hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
 
        robotArray[robot.Id] = robot
        
        # Get next input, expecting <SOH> indicating it's a new robot
        data, addr = sock.recvfrom(1024)
        hex_data = ":".join("{0:x}".format(ord(c)) for c in data)
        if (hex_data != "1"):
            sys.exit("ERROR: Expected to receive <SOH> (1 in hex) indicating it's a new robot, but received: " + str(hex_data))
            
    sock.close()
    return robotArray   